#define _USE_MATH_DEFINES

#include <math.h>
#include <stdio.h>
#include "Arm.h"

Arm::Arm(int clientID, int baseJoint, int shoulderJoint, int elbowJoint)
{
	this->k = Kinematics(10, 2, 3, 5.5);
	this->clientID = clientID;
	this->baseJoint = baseJoint;
	this->shoulderJoint = shoulderJoint;
	this->elbowJoint = elbowJoint;
	this->teta1 = 0;
	this->teta2 = 0;
	this->teta3 = 0;
	this->error = 0;
	this->delta = 0.1/180*M_PI;
	this->speed = 1;
	this->onPath = false;
	this->armON = true;
}


Arm::~Arm(void)
{
}

void Arm::goToRestPosition()
{
	onPath = false;

	error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_streaming );
	//error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_streaming );
	error = simxGetJointPosition(clientID, shoulderJoint, &teta2, simx_opmode_streaming );
	error = simxGetJointPosition(clientID, elbowJoint, &teta3, simx_opmode_streaming );
	printf("sono in teta1: %f\n", teta1);
	printf("sono in teta2: %f\n", teta2);
	printf("sono in teta3: %f\n", teta3);
		
	if(!(teta1 <= delta && teta1 >= -delta && teta2 <= delta && teta2 >= -delta && teta3 <= delta && teta3 >= -delta))
	{
		error = simxSetJointTargetVelocity(clientID, baseJoint, speed, simx_opmode_oneshot);
		error = simxSetJointTargetVelocity(clientID, shoulderJoint, speed, simx_opmode_oneshot);
		error = simxSetJointTargetVelocity(clientID, elbowJoint, speed, simx_opmode_oneshot);
		
		error = simxSetJointTargetPosition(clientID, baseJoint, 0, simx_opmode_oneshot);
		error = simxSetJointTargetPosition(clientID, shoulderJoint, 0, simx_opmode_oneshot);
		error = simxSetJointTargetPosition(clientID, elbowJoint, 0, simx_opmode_oneshot);
		printf("vado in posizione start\n");
	}
	else printf("sono in posizione start\n");
}

void Arm::goToPoint(Point p)
{
	onPath = false;

	error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_buffer);
	error = simxGetJointPosition(clientID, shoulderJoint, &teta2, simx_opmode_buffer);
	error = simxGetJointPosition(clientID, elbowJoint, &teta3, simx_opmode_buffer);
	printf("sono in teta1: %f\n", teta1);
	printf("sono in teta2: %f\n", teta2);
	printf("sono in teta3: %f\n", teta3);

	Joint j = k.inverse(p);		

	float angle_base = j.getValue(Joint::TETA_1);
	float angle_shoulder = j.getValue(Joint::TETA_2);
	float angle_elbow = j.getValue(Joint::TETA_3);

	if(!(teta1 <= angle_base+delta && teta1 >= angle_base-delta && teta2 <= angle_shoulder+delta && teta2 >= angle_shoulder-delta && teta3 <= angle_elbow+delta && teta3 >= angle_elbow-delta))
	{
		error = simxSetJointTargetVelocity(clientID, baseJoint, speed, simx_opmode_oneshot);
		error = simxSetJointTargetVelocity(clientID, shoulderJoint, speed, simx_opmode_oneshot);
		error = simxSetJointTargetVelocity(clientID, elbowJoint, speed, simx_opmode_oneshot);

		error = simxSetJointTargetPosition(clientID, baseJoint, angle_base, simx_opmode_oneshot);
		error = simxSetJointTargetPosition(clientID, shoulderJoint, angle_shoulder, simx_opmode_oneshot);
		error = simxSetJointTargetPosition(clientID, elbowJoint, angle_elbow, simx_opmode_oneshot);
		printf("vado in teta1: %f\n", angle_base);
		printf("vado in teta2: %f\n", angle_shoulder);
		printf("vado in teta3: %f\n", angle_elbow);
	}
}

void Arm::followPath(Point path[]) 
{
	onPath = true;
		for(int i = 0; sizeof(path)/sizeof(Point); i++)
		{
			if(!onPath) break;
			goToPoint(path[i]);
			onPath = true;
			Joint j = k.inverse(path[i]);
			float angle_base = j.getValue(Joint::TETA_1);
			float angle_shoulder = j.getValue(Joint::TETA_2);
			float angle_elbow = j.getValue(Joint::TETA_3);

			while(!(teta1 <= angle_base+delta && teta1 >= angle_base-delta
				&& teta2 <= angle_shoulder+delta && teta2 >= angle_shoulder-delta
				&& teta3 <= angle_elbow+delta && teta3 >= angle_elbow-delta) && onPath)
			{
				extApi_sleepMs(5);
			}
			
		}
		return;
}

void Arm::move(Joint::joint J, float degree)
{
	int joint;
	float teta = 0;

	onPath = false;

	error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_buffer);
	error = simxGetJointPosition(clientID, shoulderJoint, &teta2, simx_opmode_buffer);
	error = simxGetJointPosition(clientID, elbowJoint, &teta3, simx_opmode_buffer);
		
	switch(J){
		case 0:
			joint = baseJoint;
			teta = teta1;
		case 1:
			joint = shoulderJoint;
			teta = teta2;
		case 2:
			joint = elbowJoint;
			teta = teta3;
		default: joint = 0;
	}
	error = simxSetJointTargetPosition(clientID, joint, teta+(degree/180*M_PI), simx_opmode_oneshot);
}

void Arm::stop()
{
	speed = 0;
	onPath = false;

	error = simxSetJointTargetVelocity(clientID, baseJoint, speed, simx_opmode_oneshot);
	error = simxSetJointTargetVelocity(clientID, shoulderJoint, speed, simx_opmode_oneshot);
	error = simxSetJointTargetVelocity(clientID, elbowJoint, speed, simx_opmode_oneshot);
	printf("STOP!");
}



